#include "WPILib.h"
#include "Robot2489.h"

#define kickerReadyingTime 0.5 //time it takes for the kicker to charge OR relax OR anti-charge, should be very small
#define latchOpenTime 1 //time it takes for latch to open AND kick to actually occur.
#define kickerRetractTime 1 //time it takes for kicker piston to retract
#define latchCloseTime 1 //time it takes for latch to close so kicking will be ready again.

bool kicking = false;

	/**
	 * Runs the motors under driver control with either tank or arcade steering 
	 * selected by a jumper in DS Digin 0. 
	 * 
	 * Starts the compressor.
	 * 
	 * Enables the Watchdog.
	 * 
	 * Initializes Kicker and Latch to Relaxed and Closed respectively.
	 * 
	 * Then starts the big Operator Control while loop:
	 * 		Drives the robot using Tank Drive and the joysticks connected to ports
	 * 		1 and 2. 
	 */

	void Robot2489::OperatorControl(void)
	{
		bool strongKicking = false;
		bool mediumKicking = false;
		bool weakKicking = false;
		bool lockout = false;
		bool strongButton; //the left stick's trigger's state, pressed or not
		bool mediumButton; //the left stick's button 4's state
		bool weakButton; //the left stick's button 2's state
		bool lastStrong = false;
		bool lastMedium = false;
		bool lastWeak = false;
		bool kickerReadying = false;
		bool latchOpened = false;
		double timerState;
		
		
		KarlKompressor->Start();
		GetWatchdog().SetEnabled(true);
		RelaxKicker();
		CloseLatch();
		while (IsOperatorControl())
		{
			myRobot->TankDrive(leftStick, rightStick);
			if (!strongKicking && !mediumKicking && !weakKicking)
			{
				lockout = false;
			}
			if (leftStick->GetRawButton(8)) 
			{

				screen->Printf(DriverStationLCD::kUser_Line1, 1,
						"Z-axis value is %f", leftStick->GetZ());
				screen->UpdateLCD();
			}

			strongButton = leftStick->GetTrigger();
			if (strongButton && (lastStrong == false) && !lockout) 
			{
				strongKicking = true;
				kickTimar->Start();
				kicking = true;
				lockout = true;
			}
			lastStrong = strongButton;
			
			timerState = kickTimar->Get();
			if (strongKicking)
			{
				if (timerState < kickerReadyingTime && !kickerReadying)
				{
					ChargeKicker();
					kickerReadying = true;
				}
				if (timerState >= kickerReadyingTime && !latchOpened)
				{
					OpenLatch();
					latchOpened = true;
				}
				if (timerState >= (kickerReadyingTime + latchOpenTime) && kickerReadying)
				{
					RetractKicker();
					kickerReadying = false;
				}
				if (timerState >= (kickerReadyingTime + latchOpenTime + kickerRetractTime) && latchOpened)
				{
					CloseLatch();
					latchOpened = false;
				}
				if (timerState >= (kickerReadyingTime + latchOpenTime + kickerRetractTime + latchCloseTime))
				{
					kickTimar->Stop();
					kickTimar->Reset();
					strongKicking = false;
				}
			}
			
			mediumButton = leftStick->GetRawButton(4);
			if (mediumButton && (lastMedium == false) && !lockout) 
			{
				mediumKicking = true;
				kickTimar->Start();
				kicking = true;
				lockout = true;
			}
			lastMedium = mediumButton;
			
			if (mediumKicking)
			{				
				if (timerState < kickerReadyingTime && !kickerReadying)
				{
					RelaxKicker();
					kickerReadying = true;
				}
				if (timerState >= kickerReadyingTime && !latchOpened)
				{
					OpenLatch();
					latchOpened = true;
				}
				if (timerState >= (kickerReadyingTime + latchOpenTime) && kickerReadying)
				{
					RetractKicker();
					kickerReadying = false;
				}
				if (timerStart >= (kickerReadyingTime + latchOpenTime + kickerRetractTime) && latchOpened)
				{
					CloseLatch();
					latchOpened = false;
				}
				if (timerstate >= (kickerReadyingTime + latchOpenTime + kickerRetractTime + latchCloseTime))
				{
					kickTimar->Stop();
					kickTimar->Reset();
					mediumKicking = false;
				}
			}
			weakButton = leftStick->GetRawButton(2);
			if (weakButton && (lastWeak == false) && !lockout) 
			{
				kicking = true;
				RetractKicker(); //this ANTI-CHARGES the kicker so that the kick
								 //is totally weak when the latch releases.
				Wait(1.0);
				OpenLatch();
				Wait(4.0); //There is no explicit RetractKicker call here because
						   //after the bungie cords expend their force the earlier
						   //tendency to Retract is still in effect.
				CloseLatch();
				lockout = true;
			}	
			lastWeak = weakButton;
			if (weakKicking)
			{				
				if (timerState < kickerReadyingTime && !kickerReadying)
				{
					RetractKicker();
					kickerReadying = true;
				}
				if (timerState >= kickerReadyingTime && !latchOpened)
				{
					OpenLatch();
					latchOpened = true;
				}
				if (timerState >= (kickerReadyingTime + latchOpenTime + kickerRetractTime) && latchOpened)
				{
					CloseLatch();
					latchOpened = false;
				}
				if (timerState >= (kickerReadyingTime + latchOpenTime + kickerRetractTime + latchCloseTime))
				{
					kickTimar->Stop();
					kickTimar->Reset();
					weakKicking = false;
				}
			}
			GetWatchdog().Feed();
		}
	}
